import numpy as np
import math

def identity_matrix():
    return np.identity(4)


def transform_points(points,
                     matrix,
                     translate=True):
    points = np.asanyarray(
        points, dtype=np.float64)
    # no points no cry
    if len(points) == 0:
        return points.copy()

    matrix = np.asanyarray(matrix, dtype=np.float64)
    if (len(points.shape) != 2 or
            (points.shape[1] + 1 != matrix.shape[1])):
        raise ValueError('matrix shape ({}) doesn\'t match points ({})'.format(
            matrix.shape,
            points.shape))

    # check to see if we've been passed an identity matrix
    identity = np.abs(matrix - np.eye(matrix.shape[0])).max()
    if identity < 1e-8:
        return np.ascontiguousarray(points.copy())

    dimension = points.shape[1]
    column = np.zeros(len(points)) + int(bool(translate))
    stacked = np.column_stack((points, column))
    transformed = np.dot(matrix, stacked.T).T[:, :dimension]
    transformed = np.ascontiguousarray(transformed)
    return transformed


def concatenate_matrices(*matrices):
    M = np.identity(4)
    for i in matrices:
        M = np.dot(M, i)
    return M


def unit_vector(data, axis=None, out=None):
    """Return ndarray normalized by length, i.e. Euclidean norm, along axis.

    >>> v0 = np.random.random(3)
    >>> v1 = unit_vector(v0)
    >>> np.allclose(v1, v0 / np.linalg.norm(v0))
    True
    >>> v0 = np.random.rand(5, 4, 3)
    >>> v1 = unit_vector(v0, axis=-1)
    >>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=2)), 2)
    >>> np.allclose(v1, v2)
    True
    >>> v1 = unit_vector(v0, axis=1)
    >>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=1)), 1)
    >>> np.allclose(v1, v2)
    True
    >>> v1 = np.empty((5, 4, 3))
    >>> unit_vector(v0, axis=1, out=v1)
    >>> np.allclose(v1, v2)
    True
    >>> list(unit_vector([]))
    []
    >>> list(unit_vector([1]))
    [1.0]

    """
    if out is None:
        data = np.array(data, dtype=np.float64, copy=True)
        if data.ndim == 1:
            data /= math.sqrt(np.dot(data, data))
            return data
    else:
        if out is not data:
            out[:] = np.array(data, copy=False)
        data = out
    length = np.atleast_1d(np.sum(data * data, axis))
    np.sqrt(length, length)
    if axis is not None:
        length = np.expand_dims(length, axis)
    data /= length
    if out is None:
        return data

def rotation_matrix(angle, direction, point=None):
    """
    Return matrix to rotate about axis defined by point and
    direction.

    Parameters
    -------------
    angle     : float, or sympy.Symbol
      Angle, in radians or symbolic angle
    direction : (3,) float
      Unit vector along rotation axis
    point     : (3, ) float, or None
      Origin point of rotation axis

    Returns
    -------------
    matrix : (4, 4) float, or (4, 4) sympy.Matrix
      Homogeneous transformation matrix

    Examples
    -------------
    >>> R = rotation_matrix(math.pi/2, [0, 0, 1], [1, 0, 0])
    >>> np.allclose(np.dot(R, [0, 0, 0, 1]), [1, -1, 0, 1])
    True
    >>> angle = (random.random() - 0.5) * (2*math.pi)
    >>> direc = np.random.random(3) - 0.5
    >>> point = np.random.random(3) - 0.5
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> R1 = rotation_matrix(angle-2*math.pi, direc, point)
    >>> is_same_transform(R0, R1)
    True
    >>> R0 = rotation_matrix(angle, direc, point)
    >>> R1 = rotation_matrix(-angle, -direc, point)
    >>> is_same_transform(R0, R1)
    True
    >>> I = np.identity(4, np.float64)
    >>> np.allclose(I, rotation_matrix(math.pi*2, direc))
    True
    >>> np.allclose(2, np.trace(rotation_matrix(math.pi/2,direc,point)))
    True

    """
    if type(angle).__name__ == 'Symbol':
        # special case sympy symbolic angles
        import sympy as sp
        symbolic = True
        sina = sp.sin(angle)
        cosa = sp.cos(angle)
    else:
        symbolic = False
        sina = math.sin(angle)
        cosa = math.cos(angle)

    direction = unit_vector(direction[:3])
    # rotation matrix around unit vector
    M = np.diag([cosa, cosa, cosa, 1.0])
    M[:3, :3] += np.outer(direction, direction) * (1.0 - cosa)

    direction = direction * sina
    M[:3, :3] += np.array([[0.0, -direction[2], direction[1]],
                           [direction[2], 0.0, -direction[0]],
                           [-direction[1], direction[0], 0.0]])

    # if point is specified, rotation is not around origin
    if point is not None:
        point = np.array(point[:3], dtype=np.float64, copy=False)
        M[:3, 3] = point - np.dot(M[:3, :3], point)

    # return symbolic angles as sympy Matrix objects
    if symbolic:
        return sp.Matrix(M)

    return M

def translation_matrix(direction):
    """
    Return matrix to translate by direction vector.

    >>> v = np.random.random(3) - 0.5
    >>> np.allclose(v, translation_matrix(v)[:3, 3])
    True

    """
    # are we 2D or 3D
    dim = len(direction)
    # start with identity matrix
    M = np.identity(dim + 1)
    # apply the offset
    M[:dim, dim] = direction[:dim]
    return M
